
#include "MSE_CAN.h"
#include "MSE_DBUS.h"

CAN_FilterTypeDef CAN_FilterInitStrt;

void CAN_Filter_Init(void)
{
    CAN_FilterInitStrt.SlaveStartFilterBank = 14;
    CAN_FilterInitStrt.FilterBank = 14;
    CAN_FilterInitStrt.FilterActivation = ENABLE;
    CAN_FilterInitStrt.FilterMode = CAN_FILTERMODE_IDMASK;
    CAN_FilterInitStrt.FilterScale = CAN_FILTERSCALE_32BIT;
    CAN_FilterInitStrt.FilterIdHigh = 0x0000;
    CAN_FilterInitStrt.FilterIdLow = 0x0000;
    CAN_FilterInitStrt.FilterMaskIdHigh = 0x0000;
    CAN_FilterInitStrt.FilterMaskIdLow = 0x0000;
    CAN_FilterInitStrt.FilterBank = 0;
    CAN_FilterInitStrt.FilterFIFOAssignment = CAN_RX_FIFO0;

    HAL_CAN_ConfigFilter(&hcan1, &CAN_FilterInitStrt);
    HAL_CAN_Start(&hcan1);
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);

    CAN_FilterInitStrt.SlaveStartFilterBank = 14;
    CAN_FilterInitStrt.FilterBank = 14;

    HAL_CAN_ConfigFilter(&hcan2, &CAN_FilterInitStrt);
    HAL_CAN_Start(&hcan2);
    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);

    return;
}

void MSE_F_CAN_SEDN_UNION(CAN_HandleTypeDef *_hcan, int16_t stdid, uint8_t *Data)
{
    CAN_TxHeaderTypeDef TXMessage;

    uint32_t send_mail_box;

    TXMessage.DLC = 0x08;
    TXMessage.IDE = CAN_ID_STD;
    TXMessage.RTR = CAN_RTR_DATA;
    TXMessage.StdId = stdid;

    if ((_hcan) == &hcan1)
    {
        HAL_CAN_AddTxMessage(&hcan1, &TXMessage, Data, &send_mail_box);
    }
    else if ((_hcan) == &hcan2)
    {
        HAL_CAN_AddTxMessage(&hcan2, &TXMessage, Data, &send_mail_box);
    }
}

void MSE_F_CAN_SEDN(CAN_HandleTypeDef *_hcan, int16_t stdid, int16_t num1, int16_t num2, int16_t num3, int16_t num4)
{
    CAN_TxHeaderTypeDef TXMessage;
    uint8_t Data[8];
    uint32_t send_mail_box;

    TXMessage.DLC = 0x08;
    TXMessage.IDE = CAN_ID_STD;
    TXMessage.RTR = CAN_RTR_DATA;
    TXMessage.StdId = stdid;

    Data[0] = ((num1) >> 8);
    Data[1] = (num1);
    Data[2] = ((num2) >> 8);
    Data[3] = (num2);
    Data[4] = ((num3) >> 8);
    Data[5] = (num3);
    Data[6] = ((num4) >> 8);
    Data[7] = (num4);

    if ((_hcan) == &hcan1)
    {
        HAL_CAN_AddTxMessage(&hcan1, &TXMessage, Data, &send_mail_box);
    }
    else if ((_hcan) == &hcan2)
    {
        HAL_CAN_AddTxMessage(&hcan2, &TXMessage, Data, &send_mail_box);
    }
}
void MSE_F_CAN_SEDNSCD(CAN_HandleTypeDef *_hcan, int16_t stdid, int16_t num1, int16_t num2, int16_t num3, int16_t num4)
{
    CAN_TxHeaderTypeDef TXMessage;
    uint8_t Data[8];
    uint32_t send_mail_box;

    TXMessage.DLC = 0x08;
    TXMessage.IDE = CAN_ID_STD;
    TXMessage.RTR = CAN_RTR_DATA;
    TXMessage.StdId = stdid;

    Data[0] = ((num1) >> 8);
    Data[1] = (num1);
    Data[2] = ((num2) >> 8);
    Data[3] = (num2);
    Data[4] = ((num3) >> 8);
    Data[5] = (num3);
    Data[6] = ((num4) >> 8);
    Data[7] = (num4);

    if ((_hcan) == &hcan1)
    {
        HAL_CAN_AddTxMessage(&hcan1, &TXMessage, Data, &send_mail_box);
    }
    else if ((_hcan) == &hcan2)
    {
        HAL_CAN_AddTxMessage(&hcan2, &TXMessage, Data, &send_mail_box);
    }
}
void ctrl_motor(CAN_HandleTypeDef *hcan, uint16_t id, float _pos, float _vel)
{
    uint8_t *pbuf, *vbuf;
    pbuf = (uint8_t *)&_pos;
    vbuf = (uint8_t *)&_vel;

    CAN_TxHeaderTypeDef TXMessage;

    TXMessage.StdId = id;
    TXMessage.IDE = CAN_ID_STD;
    TXMessage.RTR = CAN_RTR_DATA;
    TXMessage.DLC = 0x08;
    uint8_t Data[8];
    uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC};

    uint32_t send_mail_box;
    Data[0] = *pbuf;
    Data[1] = *(pbuf + 1);
    Data[2] = *(pbuf + 2);
    Data[3] = *(pbuf + 3);
    Data[4] = *vbuf;
    Data[5] = *(vbuf + 1);
    Data[6] = *(vbuf + 2);
    Data[7] = *(vbuf + 3);

    if (HAL_CAN_AddTxMessage(hcan, &TXMessage, data, (uint32_t *)CAN_TX_MAILBOX0) != HAL_OK)
    {
        if (HAL_CAN_AddTxMessage(hcan, &TXMessage, data, (uint32_t *)CAN_TX_MAILBOX1) != HAL_OK)
        {
            HAL_CAN_AddTxMessage(hcan, &TXMessage, data, (uint32_t *)CAN_TX_MAILBOX2);
        }
    }
    if (HAL_CAN_AddTxMessage(hcan, &TXMessage, Data, (uint32_t *)CAN_TX_MAILBOX0) != HAL_OK)
    {
        if (HAL_CAN_AddTxMessage(hcan, &TXMessage, Data, (uint32_t *)CAN_TX_MAILBOX1) != HAL_OK)
        {
            HAL_CAN_AddTxMessage(hcan, &TXMessage, Data, (uint32_t *)CAN_TX_MAILBOX2);
        }
    }
}

CAN_RxHeaderTypeDef can_rx;
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{

    uint8_t rx_data[8];

    // 接收数据
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &can_rx, rx_data);

    if (hcan == &hcan1)
    {

        switch (can_rx.StdId)
        {

        case MSE_DF_MOTOR_HEAD_6020_YAW_ID:
            WHW_F_MOTOR_CAN_RX_6020RM(&MSE_V_MOTOR_Yaw, rx_data);
            break;
        case MSE_DF_MOTOR_HEAD_2006_L_ID:
            MSE_F_MOTOR_CAN_RX_2006RM(&MSE_V_MOTOR_L_CUP_PUMP, rx_data);
            break;
        case MSE_DF_MOTOR_HEAD_2006_R_ID:
            MSE_F_MOTOR_CAN_RX_2006RM(&MSE_V_MOTOR_R_CUP_PUMP, rx_data);
            break;
        case MSE_DF_MOTOR_HEAD_4310_PITCH_ID:
            break;
        case MSE_DF_MOTOR_HEAD_2006_SWAY_ID:
            MSE_F_MOTOR_CAN_RX_2006RM(&MSE_V_MOTOR_SWAY, rx_data);
      
            break;
        }
    }
    if (hcan == &hcan2)
    {

        switch (can_rx.StdId)
        {
          case MSE_DF_MOTOR_HEAD_3508_LIFT_ID:
            MSE_F_MOTOR_CAN_RX_2006RM(&MSE_V_MOTOR_HORIZONTAL_L, rx_data);
		  break;
		  
          case MSE_DF_MOTOR_HEAD_3508_RIGHT_ID:
            MSE_F_MOTOR_CAN_RX_2006RM(&MSE_V_MOTOR_HORIZONTAL_R, rx_data);
		  break;
        }
    }
}
